#include "calibthread.h"
#include <filesystem>
#include <fstream>
#include <chrono>
#include <QTime>

calibThread::calibThread(QObject *parent) : QThread(parent)
{
    myHandEyeCalib = new HandEyeCalib(9, 6, 0.025);
}

calibThread::~calibThread()
{

}

void calibThread::SetParams(std::vector<cv::Mat> &images, std::vector<std::vector<double>> &poses)
{
    myHandEyeCalib->calibImages_.clear();
    myHandEyeCalib->poses_.clear();

    for(int i = 0; i < images.size(); i++)
    {
        myHandEyeCalib->calibImages_.push_back(images[i]);
        Eigen::Matrix4d matrix = myHandEyeCalib->poseToHomogeneousMatrix(poses[i]);
        myHandEyeCalib->poses_.push_back(matrix);
    }
    std::cout << "myHandEyeCalib->calibImages_.size = " << myHandEyeCalib->calibImages_.size() << std::endl;
    std::cout << "myHandEyeCalib->poses_.size = " << myHandEyeCalib->poses_.size() << std::endl;
}

void calibThread::run()
{

    std::cout << "calibThread run" << std::endl;
    Eigen::Matrix4d eyeInHandT = myHandEyeCalib->computeEyeInHandT(); // 眼在手上标定
//    Eigen::Matrix4d eyeInHandT = Eigen::Matrix4d::Random();
    std::cout << "eyeInHandT = \n" << eyeInHandT << std::endl;


    sendRMS(myHandEyeCalib->rms_);

    std::vector<double> vec;
    vec.reserve(16); // 预分配空间

    // camearToend
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            vec.push_back(eyeInHandT(i, j));
        }
    }
    sendMatrix(vec);

    // intrinsic
    std::vector<double> intrinsic;
    intrinsic.reserve(4);    // fx,fy,cx,cy

    // 确保矩阵是3x3的
    if(myHandEyeCalib->cameraIntrinsicMatrix_.rows == 3 && myHandEyeCalib->cameraIntrinsicMatrix_.cols == 3) {
        intrinsic.push_back(myHandEyeCalib->cameraIntrinsicMatrix_.at<double>(0, 0));  // fx
        intrinsic.push_back(myHandEyeCalib->cameraIntrinsicMatrix_.at<double>(1, 1));  // fy
        intrinsic.push_back(myHandEyeCalib->cameraIntrinsicMatrix_.at<double>(0, 2));  // cx
        intrinsic.push_back(myHandEyeCalib->cameraIntrinsicMatrix_.at<double>(1, 2));  // cy
    } else {
        // 错误处理
        std::cerr << "Invalid camera intrinsic matrix size!" << std::endl;
        intrinsic = {0, 0, 0, 0};  // 返回默认值
    }
    sendIntrinsic(intrinsic);

}

void calibThread::saveCalibData(std::vector<cv::Mat> &images, std::vector<std::vector<double>>& poses) {
    // 获取当前时间并格式化为字符串
    auto now = std::chrono::system_clock::now();
    std::time_t now_time = std::chrono::system_clock::to_time_t(now);
    std::tm *tm_now = std::localtime(&now_time);

    // 创建文件夹名称
    std::ostringstream folderName;
    folderName << std::put_time(tm_now, "%Y%m%d_%H%M%S");

    // 创建文件夹
    std::filesystem::create_directory(folderName.str());

    // 保存图片
    for (int i = 0; i < images.size(); ++i) {
        std::string imagePath = folderName.str() + "/image_" + std::to_string(i) + ".png";
        cv::Mat image = images[i].clone();
        cv::imwrite(imagePath, image);
    }

    // 保存二维数组到 TXT 文件
    std::string posesPath = folderName.str() + "/calibPoses.txt";
    std::ofstream posesFile(posesPath);

    if (posesFile.is_open()) {
        for (const auto& pose : poses) {
            for (size_t j = 0; j < pose.size(); ++j) {
                posesFile << pose[j];
                if (j < pose.size() - 1) {
                    posesFile << " "; // 用空格分隔
                }
            }
            posesFile << "\n"; // 换行
        }
        posesFile.close();
    } else {
        std::cerr << "无法打开文件保存校准数据。" << std::endl;
    }
}
